/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/

#include "FleetFormationController.hpp"

CFleetFormationController::CFleetFormationController()
  :CSync(1,1),
   CThread(1),
   MaCI::Position::CPositionClientCallback(),
   MaCI::SpeedCtrl::CSpeedCtrlClientCallback(),
   iMasterPositionClient(NULL),
   iMasterSpeedCtrlServer(NULL),
   iMasterSpeedCtrlClient(NULL),
   iMCS(NULL),
   iMasterPosition(),
   iMasterSpeed(),
   iGimi(NULL),
   iSlavePositionClientArray(),
   iSlaveSpeedCtrlMap(),
   iSlaveRelativePositionsMap(),
   iFollow(false),
   iGoToFormation(false),
   iSlavesMaxSpeed(100.0,100.0,100.0),
   iTolerance(0.2),
   iAngleTolerance(0.05),
   iAngularSpeedRatio(2),
   iDrivingSpeed(0.2),
   iDrivingStyle(KFleetDriveFormationRelativeToMasterHeading),
   iSpeedCtrlServerThreadRunning(false),
   iSpeedCtrlServerThreadStopped(true)
{
  

  dPrint(1,"Constructor called");

}
//******************************************************************************

CFleetFormationController::~CFleetFormationController()
{

  dPrint(1,"Destructor called");

}
//******************************************************************************

bool CFleetFormationController::SetMasterMachine(MaCI::Position::CPositionClient *aMasterPositionClient,
                                                 MaCI::SpeedCtrl::CSpeedCtrlClient *aMasterSpeedCtrlClient,
                                                 std::string aMaCIGroup)
{

  dPrint(1,"Setting Master Machine");
  
 
  iMasterPositionClient = aMasterPositionClient;
 
  iMasterPositionClient->SetPositionClientCallback(this);
  
  iMasterSpeedCtrlClient = aMasterSpeedCtrlClient;
  
  iMasterSpeedCtrlClient->StartGettingSpeedEvent();
  iMasterSpeedCtrlClient->SetSpeedCtrlClientCallback(this);



    

  if(iMasterSpeedCtrlClient){

 
    //Construct gimi PTR to SpeedCtrlServer
    std::string hubAddress = aMasterPositionClient->GetGIMIPtr()->getHubAddress();
    int hubPort =aMasterPositionClient->GetGIMIPtr()->getHubPort();
    std::string gimnetName ="";

    dPrint(1,"Connecting to GIMnet for the speedctrlServer");
    iGimi = new gimi::GIMI();
    int r;
    if ( (r = iGimi->connectToHubEx(hubAddress, hubPort, gimnetName) ) != GIMI_OK) {
      dPrint(1,"Failed to connect to GIMnet AccessPoint '%s:%d' with name '%s': '%s'",
             hubAddress.c_str(), 
             hubPort,
             gimnetName.size()?gimnetName.c_str():"<anonymous>",
             gimi::getGimiErrorString(r).c_str());
      return false;
    } else {
      dPrint(1,"Succesfully connected to GIMnet at AccessPoint '%s:%d' with name '%s': '%s'",
             hubAddress.c_str(), 
             hubPort,
             gimnetName.size()?gimnetName.c_str():"<anonymous>",
             gimi::getGimiErrorString(r).c_str());

      const MaCI::MaCICtrl::SMaCISL maciSL = iMasterSpeedCtrlClient->GetDefaultTargetMaCISL();

      iMCS = new MaCI::MaCICtrl::CMaCICtrlServer(iGimi);
     
      iMCS->SetGroupName(aMaCIGroup);

      iMasterSpeedCtrlServer = new MaCI::SpeedCtrl::CSpeedCtrlServer(iMCS,-1);
      iMasterSpeedCtrlServer->SetInterfaceInstanceName("FleetCommander");

      MaCI::SpeedCtrl::TSpeed maxSpeed;
      MaCI::SpeedCtrl::TSpeed minSpeed;

      iMasterSpeedCtrlClient->GetSpeedLimits(maxSpeed,minSpeed);
      iMasterSpeedCtrlServer->SetSpeedLimits(maxSpeed.speed,maxSpeed.angularSpeed,maxSpeed.acceleration,
                                             minSpeed.speed,minSpeed.angularSpeed,minSpeed.acceleration);
      iMasterSpeedCtrlServer->Open();
      iSpeedCtrlServerThreadRunning = true;
      iSpeedCtrlServerThreadStopped = false;
      CThread::RunThread(0);
    }

  }
  
  return true;
}
//******************************************************************************

bool CFleetFormationController::GetMasterMachine(MaCI::Position::CPositionClient *aMasterPositionClient)
{
  dPrint(1,"Getting Master Machine");
  if(iMasterPositionClient){
    dPrint(1,"Master machine found");
    aMasterPositionClient = iMasterPositionClient;
    return true;
  }else{
    dPrint(1,"No master machine set");
    return false;
  }
}
//******************************************************************************

bool CFleetFormationController::AddSlaveMachine(MaCI::Position::CPositionClient *aSlavePositionClient,
                                                MaCI::SpeedCtrl::CSpeedCtrlClient *aSlaveSpeedCtrlClient,
                                                MaCI::Position::TPose2D aPose)
{


  
  iSlavePositionClientArray.push_back(aSlavePositionClient);


  iSlaveSpeedCtrlMap[aSlavePositionClient] = aSlaveSpeedCtrlClient;

  //Get the slave maximum speed.
  MaCI::SpeedCtrl::TSpeed maxSpeed,minSpeed;
  aSlaveSpeedCtrlClient->GetSpeedLimits(maxSpeed,minSpeed);
  if(iSlavesMaxSpeed.speed > maxSpeed.speed){
    iSlavesMaxSpeed.speed =  maxSpeed.speed;

  }
  if(iSlavesMaxSpeed.angularSpeed > maxSpeed.angularSpeed){
    iSlavesMaxSpeed.angularSpeed =  maxSpeed.angularSpeed;

  }
  //NOTE ACCELERATION IS NOT CORRECT
  iMasterSpeedCtrlServer->SetSpeedLimits(iSlavesMaxSpeed.speed,iSlavesMaxSpeed.angularSpeed,maxSpeed.acceleration,
                                         -iSlavesMaxSpeed.speed,-iSlavesMaxSpeed.angularSpeed,minSpeed.acceleration);

  iSlaveRelativePositionsMap[aSlavePositionClient] = aPose;
  aSlavePositionClient->SetPositionClientCallback(this);
  dPrint(1,"Added slave machine with relative position (%f,%f). Number of slaves %d", aPose.x,aPose.y,iSlavePositionClientArray.size());
  
  return true;
}
//******************************************************************************

bool CFleetFormationController::RemoveMasterMachine()
{
  if(iMasterPositionClient){
    Lock();
    iFollow = false;
    iMasterPositionClient= NULL;
    iMasterSpeedCtrlClient->StopGettingSpeedEvent();
    iMasterSpeedCtrlClient = NULL;
    dPrint(1,"Setting server thread flag to false");
    iSpeedCtrlServerThreadRunning  =false;
    Unlock();
    dPrint(1,"Setting request for termination");
    SetRequestTermination();
    dPrint(1,"Waiting thread");
    WaitThread(0);
    dPrint(1,"Closing server");
    iMasterSpeedCtrlServer->Close();
    dPrint(1,"Deleting server");
    if(iMasterSpeedCtrlServer){
      delete(iMasterSpeedCtrlServer);
    }
    if(iMCS){
      delete(iMCS);
    }
    if(iGimi){
      delete(iGimi);
    }
    dPrint(1,"Returning true");
    return true;
  }else{
    return false;
  }

}
//******************************************************************************

void CFleetFormationController::GoToFormation(){

  //iGoToFormation = true;
  iFollow = true;


}
//******************************************************************************

bool CFleetFormationController::RemoveSlaveMachine(MaCI::Position::CPositionClient *aSlavePositionClient)
{

  return false;


}
//******************************************************************************

bool CFleetFormationController::RemoveAllSlaves()
{
  if(iSlavePositionClientArray.size() >0){
    iSlavePositionClientArray.erase(iSlavePositionClientArray.begin(),iSlavePositionClientArray.end());
    iSlaveSpeedCtrlMap.erase(iSlaveSpeedCtrlMap.begin(),iSlaveSpeedCtrlMap.end());
    iSlaveRelativePositionsMap.erase(iSlaveRelativePositionsMap.begin(),iSlaveRelativePositionsMap.end());
    return true;
  }else{
    return false;
  }

}
//******************************************************************************

void CFleetFormationController::StartToFollow()
{
  iFollow = true;
}
//******************************************************************************

void CFleetFormationController::StopToFollow()
{
  iFollow = false;
}
//******************************************************************************

void CFleetFormationController::OnPositionDataEvent(const MaCI::Position::CPositionData &aData, 
                                                    const unsigned int aDataSequence,
                                                    MaCI::Position::CPositionClient &aPositionClient,
                                                    void * aUserPtr){


  if(iFollow || iGoToFormation){
    if(&aPositionClient == iMasterPositionClient){
      
      Lock();
      iMasterPosition = *(aData.GetPose2D());
      Unlock();
      
    }else{
      bool slaveClient= false;
      for(unsigned int i = 0; i< iSlavePositionClientArray.size(); i++){      
        if(iSlavePositionClientArray[i] == &aPositionClient){
          slaveClient = true;
          break;
        }
      }
      //SLAVE MACHINE
      if(slaveClient){
        DoSlaveControlling(&aPositionClient,
                           aData.GetPose2D());
      }
      
    }
  }
  
  
}

//******************************************************************************
void CFleetFormationController::DoSlaveControlling(MaCI::Position::CPositionClient *aPositionClient,
                                                   const MaCI::Position::TPose2D *aSlaveCurrentPose){


  /*  if(iGoToFormation){

    GoToFormation(aPositionClient, aSlaveCurrentPose);
    return;
    }*/
  
  if(iDrivingStyle == KFleetDriveFormationStaysStatic){
    DoFormationStaticDriving(aPositionClient,aSlaveCurrentPose);
  }else if (iDrivingStyle == KFleetDriveFormationRelativeToMasterHeading){
    
    DoFormationRelativeToMasterHeading(aPositionClient,aSlaveCurrentPose);
  }


}
//******************************************************************************

float CFleetFormationController::NormalizeAngle(float aAngle)
{
  float angle = aAngle;
  if(angle > M_PI){
      while(angle >  M_PI){
	 angle-= 2.0 * M_PI;
      }
  }
  else if(angle < -M_PI){
    while(angle < -M_PI){
      angle += 2.0 * M_PI;
    }	
  }
  return angle;
}
//*****************************************************************************
//******************************************************************************
void CFleetFormationController::OnSpeedCtrlDataEvent(const MaCI::SpeedCtrl::CSpeedCtrlData &aData, 
                                                     const unsigned int aDataSequence,
                                                     MaCI::SpeedCtrl::CSpeedCtrlClient &aSpeedCtrlClient,
                                                     void * aUserPtr)
{
  // dPrint(1,"Got master speed event");
  Lock();
  const  MaCI::SpeedCtrl::TSpeed *speed = aData.GetSpeed();
  if(speed){
    //FIXME:: Maybe later - Now the set value is copied here
			//iMasterSpeed = *speed;
    //dPrint(1,"Master speed is %f %f %f", iMasterSpeed.speed,iMasterSpeed.angularSpeed,iMasterSpeed.acceleration);
  }
  Unlock();

}

//******************************************************************************

void CFleetFormationController::GoToFormation(MaCI::Position::CPositionClient *aPositionClient,
                                              const MaCI::Position::TPose2D *aSlaveCurrentPose)
{



}
//******************************************************************************

/*bool CFleetFormationController::IsInFormation(){
  bool returnvalue = true;
  
  std::map<MaCI::Position::CPositionClient*, bool> iSlaveInFormation;

  return returnvalue;
  }*/
//******************************************************************************

void CFleetFormationController::SetFleetDrivingStyle(EFleetDrivingStyle aStyle)
{
  iDrivingStyle = aStyle;

}
//******************************************************************************
void CFleetFormationController::DoFormationStaticDriving(MaCI::Position::CPositionClient *aPositionClient,
                                                         const MaCI::Position::TPose2D *aSlaveCurrentPose)
{



  //dPrint(1,"Starting calc");
     
  Lock();
  MaCI::Position::TPose2D slaveRelativePosition = iSlaveRelativePositionsMap[aPositionClient];
  MaCI::SpeedCtrl::TSpeed masterSpeed = iMasterSpeed;
  float slaveTargetAngle = iMasterPosition.a;
  
  //testing :)

  float slaveTargetX = iMasterPosition.x + slaveRelativePosition.x;
  float slaveTargetY = iMasterPosition.y + slaveRelativePosition.y;
  Unlock();
  float	rotation = atan2(slaveTargetY - aSlaveCurrentPose->y,
                         slaveTargetX - aSlaveCurrentPose->x);
  

  float deltaX = slaveTargetX -aSlaveCurrentPose->x;
  float deltaY = slaveTargetY -aSlaveCurrentPose->y;
  float deltaA = slaveTargetAngle -aSlaveCurrentPose->a;
  float distance = sqrt(pow(deltaX,2)+pow(deltaY,2));

  float angleToGoal = rotation-aSlaveCurrentPose->a;
  angleToGoal =   NormalizeAngle(angleToGoal);
  deltaA =   NormalizeAngle(deltaA);
  float speed = 0;
  float angularSpeed = 0;
  float acceleration = 0.2;
  float maxSpeedDistance = 1.0;

  float p = 1.2;
  float forwardDistance = cos(angleToGoal) * distance;
 
  if(distance > iTolerance){
    //dPrint(1,"Distance > tolerance");
    
    angularSpeed = masterSpeed.angularSpeed + (angleToGoal)/iAngularSpeedRatio;
    dPrint(1,"angularSpeed %f master aspeed %f, angleToGoal %f",angularSpeed,masterSpeed.angularSpeed,angleToGoal);
    if(fabs(angleToGoal) < M_PI/4 ){
      if(fabs(masterSpeed.speed) > 0.01){
        //        speed = masterSpeed.speed *( 1+(forwardDistance)/maxSpeedDistance);
        //Master is going backwards then slow down if the goal is ahead, and accelerate if
        // it is behind..
        if(masterSpeed.speed < 0.0){
          forwardDistance = -forwardDistance;
        }
        speed = masterSpeed.speed * (1+p*forwardDistance);

      }else{
        //speed = iSlavesMaxSpeed.speed*( 1+(forwardDistance)/maxSpeedDistance);
        speed = iSlavesMaxSpeed.speed*((forwardDistance)/maxSpeedDistance);
      }
    }
    //dPrint(1,"NOT. Forward distance is %f, speed %f",forwardDistance,speed);
    
  }else{
    //If we are inside the tolerance area

    //speed = masterSpeed.speed *( 1+(forwardDistance)/maxSpeedDistance);
    //Master is going backwards then slow down if the goal is ahead, and accelerate if
    // it is behind..
    if(masterSpeed.speed < 0.0){
      forwardDistance = -forwardDistance;
    }
    speed = masterSpeed.speed * (1+p*forwardDistance);
    dPrint(100,"masterSpeed %f speed %f, forwardDistance %f",masterSpeed.speed,speed,forwardDistance);
    angularSpeed = masterSpeed.angularSpeed + deltaA/iAngularSpeedRatio;
    dPrint(10,"angularSpeed %f master aspeed %f, deltaA %f",angularSpeed,masterSpeed.angularSpeed,deltaA);
    // dPrint(1,"IN. Forward distance is %f, speed %f",forwardDistance,speed);
      
  }
  if(speed > iSlavesMaxSpeed.speed){
    speed = iSlavesMaxSpeed.speed-0.01;
  }

  if(angularSpeed > iSlavesMaxSpeed.angularSpeed){

    angularSpeed =iSlavesMaxSpeed.angularSpeed;
  }
  if(angularSpeed < -iSlavesMaxSpeed.angularSpeed){

    angularSpeed =-iSlavesMaxSpeed.angularSpeed;
  }
 
  dPrint(10,"Client %p, position (%f, %f) angle %f, slaveTarget (%f,%f) a %f, delta (%f,%f) deltaA %f, distance %f, tolerance %f, angleToGoal %f, Setting speed %f,%f, Master speed %f, %f",
         aPositionClient,
         aSlaveCurrentPose->x,
         aSlaveCurrentPose->y,
         aSlaveCurrentPose->a,
         slaveTargetX,
         slaveTargetY,
         slaveTargetAngle,
         deltaX,
         deltaY,
         deltaA,
         distance,
         iTolerance,
         angleToGoal,
         speed,
         angularSpeed,
         masterSpeed.speed,
         masterSpeed.angularSpeed);

  dPrint(10,"MASTER (%.2f, %.2f) Setting speed (%f %f) acceleration %f to '%s'",masterSpeed.speed,
         masterSpeed.angularSpeed, speed,angularSpeed,acceleration,iSlaveSpeedCtrlMap[aPositionClient]->GetDefaultTargetMaCISL().group.c_str());
  iSlaveSpeedCtrlMap[aPositionClient]->SetSpeed(speed,angularSpeed,acceleration);
}
//******************************************************************************
void  CFleetFormationController::DoFormationRelativeToMasterHeading(MaCI::Position::CPositionClient *aPositionClient,
                                                                    const MaCI::Position::TPose2D *aSlaveCurrentPose)
{

  //dPrint(1,"Starting calc");
  Lock();
  MaCI::Position::TPose2D slaveRelativePosition = iSlaveRelativePositionsMap[aPositionClient]; ///Suhteellinen tavoitepiste
  MaCI::SpeedCtrl::TSpeed masterSpeed = iMasterSpeed; ///Masterin nykyinen (tavoite)nopeus ja kulmanopeus
  float mx = iMasterPosition.x; ///master x
  float my = iMasterPosition.y; //master y
  float ma = iMasterPosition.a; ///master heading
  
  float px = aSlaveCurrentPose->x ;
	float py = aSlaveCurrentPose->y ;
	float pa = aSlaveCurrentPose->a;
	Unlock();
	
	///Prediction of next step
	float dt = 0.1; ///good sophisticated guess
	float mx1 = mx + masterSpeed.speed * dt * cos(ma);
	float my1 = my + masterSpeed.speed * dt * sin(ma);
	float ma1 = ma + masterSpeed.angularSpeed*dt;
	
	///Target for the Slave
	float stx = slaveRelativePosition.x * cos(ma1) - slaveRelativePosition.y * sin(ma1) + mx1;
	float sty = slaveRelativePosition.x * sin(ma1) + slaveRelativePosition.y * cos(ma1) + my1;
	float sta = ma1;
	/////////////////////////////////////////////////////////////////////////////////////////////
	/**
	void setToDifferentialPose(pose odo_cur,pose odo_ref){					
		float ddx,ddy,dist,alpha;
		pose tmp;
		///Calculates the differential movement in odometry frame of reference
		ddx = odo_cur.x - odo_ref.x;
		ddy = odo_cur.y - odo_ref.y;
		alpha = atan2(ddy, ddx);
		dist = sqrt(ddx*ddx+ddy*ddy);
		
		tmp.x = dist * cos( alpha - odo_ref.a );
		tmp.y = dist * sin( alpha - odo_ref.a );
		tmp.a = (float)fmod((float)(odo_cur.a - odo_ref.a), (float)(2.0*M_PI)); 
		if(tmp.a < 0) tmp.a += 2*(float)M_PI;
		x = tmp.x;
		y= tmp.y;
		a = tmp.a;		
	**/
	/////////////////////////////////////////////////////////////////////////////////////////////
	float ddx,ddy,dist,alpha;
	ddx = -px + stx;
	ddy = -py + sty;
	alpha = atan2(ddy, ddx);
	dist = sqrt(ddx*ddx+ddy*ddy);
	
	float tmpx = dist * cos( alpha - pa );
	float tmpy = dist * sin( alpha - pa );
	float tmpa = (sta - pa);
	tmpa = NormalizeAngle(tmpa);
	/////////////////////////////////////////////////////////////////////////////////////////////
	///Target speed 
	float vTarget = 0.5 * (tmpx / fabs(tmpx) ) * dist + masterSpeed.speed; 
	float wTarget =0.5 * tmpa;
	///Incorporate "Yerror"
	if(fabs(ddy) > 0.05){
		alpha = NormalizeAngle(alpha);
		wTarget = 0.1 * alpha;
	}
	
	
 	if(masterSpeed.speed == 0.0 && masterSpeed.angularSpeed==0.0 && dist < 0.1 && fabs(tmpa)<(10.0*M_PI/180.0)){
 		vTarget *= 0.1;
 		wTarget *= 0.1;
 	}else if(masterSpeed.speed == 0.0 && masterSpeed.angularSpeed==0.0 && dist < 0.04 && fabs(tmpa)<(2.0*M_PI/180.0)){
 			vTarget *= 0.05;
 		wTarget *= 0.05;
 	}
		

	float vr,vl;
	float rRobot = 0.06;
	vl = vTarget-rRobot*wTarget;
	vr = rRobot * wTarget + vTarget;
	
	float vRobot = 0.15;
	if(fabs(vl) > 0.15 || fabs(vr) > 0.15){
		dPrint(1,"Master too far or too fast vl,vr = (%.3f, %.3f)\n ", vl,vr);
		float vScale;
		if(fabs(vl) > fabs(vr)){
			vScale = vRobot/fabs(vl);
		}else{
			vScale = vRobot/fabs(vr);
		}
		
		vl *= vScale;
		vr *= vScale; 
		
		
		vTarget = (vl+vr)/2.0;
		wTarget = (vr-vl)/(2.0*rRobot);
	}
	float acceleration = 0.1;
	fprintf(stderr,"(Slave A=%.2f alpha=%.2f) V=%.2f W=%.2f, Master = (%.2f %.2f) : dist=%.2f, ddx=%.2f, ddy=%.2f T:(%.2f %.2f %.2f) Pos=T:(%.2f %.2f %.2f)\n",slaveRelativePosition.a, alpha,vTarget, wTarget,masterSpeed.speed, masterSpeed.angularSpeed, dist, ddx, ddy, stx,sty,sta,px,py,pa );
  iSlaveSpeedCtrlMap[aPositionClient]->SetSpeed(vTarget,wTarget,acceleration);
}

//******************************************************************************

int CFleetFormationController::ThreadFunction(const int aThreadNumber){

  switch(aThreadNumber){
  case 0:
    return SpeedCtrlServerThread();
    break;
  default:
    dPrint(1,"Invalid thread number!");
    return 0;
    break;
  }

}
//******************************************************************************

int CFleetFormationController::SpeedCtrlServerThread(){

  MaCI::EMaCIError e;
  while(iSpeedCtrlServerThreadRunning){
    using namespace MaCI::SpeedCtrl;
    
    // Construct Data element
    CSpeedCtrlData cdata;


    e = iMasterSpeedCtrlServer->WaitCommand(cdata, 1000, true);
    // Got command? (No timeout or error)
    if (e == MaCI::KMaCIOK) {
      const TCommand *cmd = cdata.GetCommand();
      // Switch based on command type
      switch(cmd->cmd) {
      case KCommandSetStop: {
        // Got command SetStop
        iMasterSpeedCtrlClient->SetStop();
				iMasterSpeed = MaCI::SpeedCtrl::TSpeed(0,0,0);
        break;
      }
        
      case KCommandSetSpeed: {
        // Got speed command?
        const TSpeed *tspeed = cdata.GetSpeed();
        if (tspeed) {
          float speed = tspeed->speed;
          float angularSpeed = tspeed->angularSpeed;
          float acceleration = tspeed->acceleration;
          if(speed > iSlavesMaxSpeed.speed){
            speed = iSlavesMaxSpeed.speed;
          }

          if(angularSpeed > iSlavesMaxSpeed.angularSpeed){
            angularSpeed = iSlavesMaxSpeed.angularSpeed;
          }

          if(acceleration > iSlavesMaxSpeed.acceleration){
            acceleration = iSlavesMaxSpeed.acceleration;
          }
          ///FIXME:: REDUCING MASTER SPEED NOW
          speed=0.8*speed;
					angularSpeed = 0.4*angularSpeed;
					iMasterSpeed = MaCI::SpeedCtrl::TSpeed(speed,angularSpeed,acceleration);
					
          // Got command SetSpeed()
          iMasterSpeedCtrlClient->SetSpeed(speed,angularSpeed,acceleration);
          
        } else {
          dPrint(1,"WARNING: No speed element available even SetSpeed was commanded!");
          
        }
        break;
      }
      case KCommandTimeout:{
        dPrint(1,"ALERT! No command within timeout. Calling SetStop!");
        iMasterSpeedCtrlClient->SetStop();
				iMasterSpeed = MaCI::SpeedCtrl::TSpeed(0,0,0);
        break;
      }
      default:
        dPrint(1,"No implementation for command %d!", cmd->cmd);
        break;
      }
      
      
      
    }
  }
  iSpeedCtrlServerThreadStopped = true;
  return 1;
}
//******************************************************************************
//******************************************************************************
//******************************************************************************
//******************************************************************************
//******************************************************************************
